#include "adc.h"
#include <Arduino.h>

ADC_HandleTypeDef hadc1;
volatile uint32_t iu_raw, iv_raw, iw_raw;
volatile uint16_t vdc_raw = 0;  // DC bus voltage reading
volatile uint16_t speedref_raw = 0;  // Speed reference reading

void ADC1_Init(void)
{
    // Enable clocks
    __HAL_RCC_ADC12_CLK_ENABLE();    
    __HAL_RCC_GPIOA_CLK_ENABLE();    
    __HAL_RCC_GPIOB_CLK_ENABLE();    
    __HAL_RCC_GPIOC_CLK_ENABLE();    

    // Configure GPIO pins
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;    // Set pins to analog mode
    GPIO_InitStruct.Pull = GPIO_NOPULL;         // No pull-up/pull-down

    // Configure PA0 as analog input for DC bus voltage
    GPIO_InitStruct.Pin = GPIO_PIN_0;           // PA0 for DC bus voltage
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = GPIO_PIN_1;           // PA1 for phase U
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1;  // PB0,PB1 for phase W,V
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    // Configure PC2 for speed reference
    GPIO_InitStruct.Pin = GPIO_PIN_2;           // PC2 for speed ref
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

    // Configure ADC
    hadc1.Instance = ADC1;
    hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
    hadc1.Init.Resolution = ADC_RESOLUTION_12B;
    hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
    hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE;      // Changed to disable scan
    hadc1.Init.EOCSelection = ADC_EOC_SEQ_CONV;   // Single conversion mode
    hadc1.Init.LowPowerAutoWait = DISABLE;
    hadc1.Init.ContinuousConvMode = DISABLE;
    hadc1.Init.NbrOfConversion = 1;                  // Changed to 1 conversion
    hadc1.Init.DiscontinuousConvMode = DISABLE;
    hadc1.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
    hadc1.Init.OversamplingMode = DISABLE;  // Add this
    HAL_ADC_Init(&hadc1);

    // Configure base channel settings
    ADC_ChannelConfTypeDef sConfig = {0};
    sConfig.SingleDiff = ADC_SINGLE_ENDED;
    sConfig.OffsetNumber = ADC_OFFSET_NONE;
    sConfig.Offset = 0;
    sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5;
    sConfig.Rank = ADC_REGULAR_RANK_1;

    // Configure initial channel (VDC)
    sConfig.Channel = ADC_CHANNEL_1;
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);

    // Configure channels for injected mode
    ADC_InjectionConfTypeDef sConfigInjected = {0};
    sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_6CYCLES_5;    // 6.5 cycle sampling time
    sConfigInjected.InjectedNbrOfConversion = 3;                        // 3 injected conversions
    sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;           // No discontinuous mode
    sConfigInjected.AutoInjectedConv = DISABLE;                        // No auto injection
    sConfigInjected.QueueInjectedContext = DISABLE;                    // No queue mode
    sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED;  // Add this line to configure single-ended mode
    
    // Configure U phase current (PA1 - ADC1_IN2)
    sConfigInjected.InjectedChannel = ADC_CHANNEL_2;                   // Channel 2
    sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;               // First conversion
    HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected);

    // Configure V phase current (PB1 - ADC1_IN12)
    sConfigInjected.InjectedChannel = ADC_CHANNEL_12;                  // Channel 12
    sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;               // Second conversion
    HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected);

    // Configure W phase current (PB0 - ADC1_IN15)
    sConfigInjected.InjectedChannel = ADC_CHANNEL_15;                  // Channel 15
    sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;               // Third conversion
    HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected);

    // Configure injected channels sequence length
    hadc1.Instance->JSQR &= ~ADC_JSQR_JL;  // Clear sequence length bits
    hadc1.Instance->JSQR |= ((3-1) << ADC_JSQR_JL_Pos);  // Set sequence length to 3 conversions

    // Configure TIM1 TRGO trigger
    hadc1.Instance->JSQR &= ~(ADC_JSQR_JEXTSEL | ADC_JSQR_JEXTEN);
    hadc1.Instance->JSQR |= ADC_EXTERNALTRIGINJEC_T1_TRGO | 
                           ADC_EXTERNALTRIGINJECCONV_EDGE_RISING;

    // Configure ADC interrupt
    HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(ADC1_2_IRQn);

    // Clear and enable only JEOS interrupt
    ADC1->ISR = ADC_ISR_JEOS;
    ADC1->IER = ADC_IER_JEOSIE;

    // Start ADC
    HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);  // Specify single-ended for calibration
    HAL_ADC_Start(&hadc1);
    ADC1->CR |= ADC_CR_JADSTART;
}

void ADC1_ReadValues(void)
{
    static uint8_t channel = 0;
    ADC_ChannelConfTypeDef sConfig = {0};
    sConfig.Rank = ADC_REGULAR_RANK_1;
    sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5;
    sConfig.SingleDiff = ADC_SINGLE_ENDED;
    sConfig.OffsetNumber = ADC_OFFSET_NONE;
    sConfig.Offset = 0;

    // Alternate between channels
    if(channel == 0) {
        sConfig.Channel = ADC_CHANNEL_1;  // VDC
        HAL_ADC_ConfigChannel(&hadc1, &sConfig);
        HAL_ADC_Start(&hadc1);
        if(HAL_ADC_PollForConversion(&hadc1, 1) == HAL_OK) {
            vdc_raw = HAL_ADC_GetValue(&hadc1);
        }
        channel = 1;
    } else {
        sConfig.Channel = ADC_CHANNEL_8;  // Speed ref
        HAL_ADC_ConfigChannel(&hadc1, &sConfig);
        HAL_ADC_Start(&hadc1);
        if(HAL_ADC_PollForConversion(&hadc1, 1) == HAL_OK) {
            speedref_raw = HAL_ADC_GetValue(&hadc1);
        }
        channel = 0;
    }
}

// Only ADC IRQ Handler needed here
void ADC1_2_IRQHandler(void)
{
    HAL_ADC_IRQHandler(&hadc1);
}

